/////////////////////////////////////////////////////////////////////////
//  Diversity controller by MonZon (july 2013)
//  MonVersity v2.0
//  (c) MonZon
//  Use at your own risk!
//////////////////////////////////////////////////////////////////////////
#define STARTUP  3  //delay in seconds
#define THRESHOLD  3  //switch if bigger than this value
#define SAMPLES  10  //number of samples for sliding average filter

//Hardware definitions here
//RX leds
#define LED1 15
#define LED2 16
#define SYSLED 13  //led on arduino board

//UNCOMMENT AND PUT YOUR BUZZER PIN HERE
//#define BUZZER 14  

//UNCOMMENT TO ENABLE SERIAL LOGGING
#define LOGGING 1

//video sync inputs
#define RX1  5
#define RX2  6

//manual override switch
#define SWITCH1  7
#define SWITCH2  8

//choose video switch pin
#define SELECTOR  9
////////////////////////////////////////////////////////////////

#include <MovingAvarageFilter.h>

MovingAvarageFilter buffer1(SAMPLES);
MovingAvarageFilter buffer2(SAMPLES);


//rssi values stored here
unsigned int rx1=0;
unsigned int rx2=0;
boolean activerx=0;

unsigned long previousMillis = 0;        // will store last time RX was set
unsigned long interval = 50;           // interval (milliseconds)


#ifdef BUZZER
void beep(unsigned int duration)
{
  digitalWrite(BUZZER, HIGH);
  if (duration>500) duration=400; //buzzing time must be lower than 400ms
  delay(duration);  
  digitalWrite(BUZZER, LOW);  
}
#endif



//switch to another rx or not if same
void switch_rx(boolean rx)
{ 
  
#ifdef LOGGING    
    Serial.print(rx1);
    Serial.print(' ');
    Serial.println(rx2);
#endif

  if (activerx!=rx) 
  {
    digitalWrite(SELECTOR,rx); 
    activerx=rx; 
    digitalWrite(LED1,rx);
    digitalWrite(LED2,!rx);

#ifdef BUZZER
    beep(100);
#endif 
  }
}


void rx1_cnt()
{
rx1++;  
}

void rx2_cnt()
{
rx2++;  
}


void setup()  //hardware setup
{

#ifdef LOGGING     
  // initialize serial communication with computer:
  Serial.begin(115200);                   
  Serial.println("MonVersity v2.0 by MonZon\nBoot OK");
#endif

  delay(STARTUP*1000);  //wait before loading
  
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(SYSLED, OUTPUT);
  pinMode(SELECTOR, OUTPUT);

#ifdef BUZZER
  pinMode(BUZZER, OUTPUT);
  digitalWrite(BUZZER, LOW);
#endif  

  pinMode(RX1, INPUT);
  pinMode(RX2, INPUT); 
  pinMode(SWITCH1, INPUT);
  pinMode(SWITCH2, INPUT); 
  digitalWrite(SWITCH1, HIGH);  //pull-up resistors enabled
  digitalWrite(SWITCH1, HIGH);
  attachInterrupt(0,rx1_cnt,FALLING);  //video sync counter interrupts
  attachInterrupt(1,rx2_cnt,FALLING);
#ifdef LOGGING     
  Serial.print("Setup OK\nMax switching per second = "); 
  Serial.println(1000/interval);
  Serial.println("Testing");
  
#endif

  switch_rx(0);
  digitalWrite(SYSLED, 1);
#ifdef BUZZER
  beep(400);
  beep(400);
  beep(400);
#endif 
  delay(1000);
  switch_rx(1);
  digitalWrite(SYSLED, 0);
  delay(1000);
  switch_rx(0);
#ifdef LOGGING     
  Serial.println("Test OK\nSTARTED"); 
#endif

  rx1=0;
  rx2=0;
  digitalWrite(SYSLED, 1);
  
}

boolean read_switch()
{
  if ((digitalRead(SWITCH1)||digitalRead(SWITCH2))==1) return 0; //auto mode
  else return 1;  //manual override
}

boolean ledState = LOW;             // ledState used to set the system LED


void loop()
{
  //Main loop

    unsigned long currentMillis = millis();  //read current time


  if (currentMillis - previousMillis > interval) //interval checking

  {
    previousMillis = currentMillis;  //save current time

    if (read_switch()==0)  //check for manual override
      //checking last active rx and switching if needed
      {
      switch_rx((rx2-rx1)>=THRESHOLD);  //switch to best rx in auto mode             
        noInterrupts();
            // critical, time-sensitive code here
      rx1=0;
      rx2=0;
        interrupts();
        
    if (ledState == LOW) ledState = HIGH; else ledState = LOW;
    digitalWrite(SYSLED, ledState);  //blink system led
      }
    else {
#ifdef LOGGING   
          Serial.print("MANUAL=");
          Serial.println(SWITCH1+1); //Manual override to X
#endif          
      if (digitalRead(SWITCH1)==0) 
           switch_rx(0);  //switch to chosen rx
            else switch_rx(1);
        
 
    }

 
  }//end interval routine

  delay(1);        // delay for stability   



}





